#include "context.h"
#include "globals.h"
#include "pcbqueue.h"

VOID process_switch() {
	PCB* next_pcb = rpq_dequeue();

	context_switch( current_process, next_pcb );
}

VOID i_process_switch() {
	save_proc->process_state = READY;
	rpq_enqueue(save_proc);
	save_proc = NULL;

	process_switch();
}

VOID context_switch(PCB* current, PCB* next) {
	UINT32 temp1;
	UINT32 temp2;
	UINT32 temp3;

	if(current == next) {
		asm( "unlk %a6" );

		asm( "move.l %d1, -(%a7)" );
		asm( "move.l #16, %d1" );
		asm( "move.w #0x4567, %d0" );
		asm( "lsl.l %d1, %d0" );
		asm( "move.l (%a7)+, %d1" );
		asm( "move.w %SR, %d0" );
		asm( "move.l %d0, -(%a7)" );

	} else {
		if(current != NULL) {
			asm( "move.l 4(%a6), %d0" );
			asm( "move.l %d0, -4(%a6)" );
			current->program_counter = (UINT32*)temp1;
			asm( "move.l %d1, -(%a7)" );
			asm( "move.l #16, %d1" );
			asm( "move.w #0x4567, %d0" );
			asm( "lsl.l %d1, %d0" );
			asm( "move.l (%a7)+, %d1" );
			asm( "move.w %SR, %d0" );
			asm( "move.l %d0, -8(%a6)" );
			current->status_register = temp2;

			asm( "move.l %d0, -(%a7)" );
			asm( "move.l %d1, -(%a7)" );
			asm( "move.l %d2, -(%a7)" );
			asm( "move.l %d3, -(%a7)" );
			asm( "move.l %d4, -(%a7)" );
			asm( "move.l %d5, -(%a7)" );
			asm( "move.l %d6, -(%a7)" );
			asm( "move.l %d7, -(%a7)" );
			asm( "move.l %a0, -(%a7)" );
			asm( "move.l %a1, -(%a7)" );
			asm( "move.l %a2, -(%a7)" );
			asm( "move.l %a3, -(%a7)" );
			asm( "move.l %a4, -(%a7)" );
			asm( "move.l %a5, -(%a7)" );

			asm( "move.l %a7, -4(%a6)" );
			current->stack_pointer = (UINT32*)temp1;
		}

		if(next->process_state != I_PROCESS) {
			next->process_state = EXECUTING;
		}

		current_process = next;

		temp1 = (UINT32)next->program_counter;
		temp2 = next->status_register;
		temp3 = (UINT32)next->stack_pointer;
		asm( "move.l -12(%a6), %a7" );

		asm( "move.l (%a7)+, %a5" );
		asm( "move.l (%a7)+, %a4" );
		asm( "move.l (%a7)+, %a3" );
		asm( "move.l (%a7)+, %a2" );
		asm( "move.l (%a7)+, %a1" );
		asm( "move.l (%a7)+, %a0" );
		asm( "move.l (%a7)+, %d7" );
		asm( "move.l (%a7)+, %d6" );
		asm( "move.l (%a7)+, %d5" );
		asm( "move.l (%a7)+, %d4" );
		asm( "move.l (%a7)+, %d3" );
		asm( "move.l (%a7)+, %d2" );
		asm( "move.l (%a7)+, %d1" );
		asm( "move.l (%a7)+, %d0" );

		asm( "move.l #0, (%a7)+" );
		asm( "move.l #0, (%a7)+" );
		asm( "move.l #0, (%a7)+" );
		asm( "move.l (%a7)+, -12(%a6)" );
		asm( "move.l #0, (%a7)+" );

		asm( "move.l -4(%a6), %d0" );
		asm( "move.l %d0, -(%a7)" );
		asm( "move.l -8(%a6), %d0" );
		asm( "move.l %d0, -(%a7)" );

		asm( "move.l -12(%a6), %a6" );
	}

	asm( "rte" );
}
